#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32.hpp"
#include "std_srvs/srv/set_bool.hpp"

#include "water_tank/water_tank.hpp"

class WaterTankControlNode : public rclcpp::Node {
public:
  WaterTankControlNode(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "Node %s has been started.", name.c_str());

    // 水位订阅者初始化
    water_level_subscription_ =
        this->create_subscription<std_msgs::msg::Float32>(
            "water_level", 10,
            [&](const std_msgs::msg::Float32::SharedPtr msg) {
              waterLevelCallback(msg);
            });

    // 进水服务客户端初始化
    water_in_client_ =
        this->create_client<std_srvs::srv::SetBool>("set_water_in");
  }

private:
  // 最低水位
  const float min_water_level_ = 20.0;
  // 水箱水位订阅者
  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr
      water_level_subscription_;
  // 进水服务客户端
  rclcpp::Client<std_srvs::srv::SetBool>::SharedPtr water_in_client_;

  // 水位订阅回调函数
  void waterLevelCallback(const std_msgs::msg::Float32::SharedPtr msg) {
    RCLCPP_INFO(this->get_logger(), "Current water level: %.2f", msg->data);
    if (msg->data < min_water_level_) {
      // 水位低于最低水位，发送进水请求
      auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
      request->data = true;
      auto result_future = water_in_client_->async_send_request(
          request,
          [&](rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture future) {
            handleWaterInResponse(future);
          });
    }
  }

  // 进水服务响应回调函数
  void handleWaterInResponse(
      rclcpp::Client<std_srvs::srv::SetBool>::SharedFuture future) {
    auto response = future.get();
    if (response->success) {
      RCLCPP_INFO(this->get_logger(), "Water in service succeeded: %s",
                  response->message.c_str());
    } else {
      RCLCPP_WARN(this->get_logger(), "Water in service failed: %s",
                  response->message.c_str());
    }
  }
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<WaterTankControlNode>("water_tank_control_node");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}